#include "stm32f10x.h"
#include "stm32f10x_tim.h"
#include "imu.h"
#include "MPU6050.h"
#include "math.h"
#include "delay.h"
#include "FreeRTOS.h"
#include "task.h"
#define deg2rad_factor (3.14159265358979323846f / 180.0f)
#define rad2deg_factor (180.0f / 3.14159265358979323846f)
MPU6050_Data mpu_data;
static MPU6050_Data mpu_data_raw;
static MPU6050_Data mpu_data_bias;
static MPU6050_Data mpu_data_calibrated;
IMU_data imu_data = {
    .pitch = 0.0f,
    .roll = 0.0f,
    .yaw = 0.0f
};
MahonyData_t mahony_data = {
    .q0 = 1.0f,
    .q1 = 0.0f,
    .q2 = 0.0f,
    .q3 = 0.0f,
    .ex = 0.0f,
    .ey = 0.0f,
    .ez = 0.0f,
    .k1 = 0.001f, // 假设比例常数 k1
    .k2 = 20.0f,  // 假设比例常数 k2
    .alpha = 0.9f,
    .integral_limit = 0.1f
};


uint8_t flag_first = 1;

void MahonyUpdate(MPU6050_Data* data, float dt)
{
    float ax, ay, az;
    float gx, gy, gz;
    float ax_filt, ay_filt, az_filt;
    float norm;

    // 单位转换（假设已经考虑比例因子）
    gx = data->gyro_x * deg2rad_factor;
    gy = data->gyro_y * deg2rad_factor;
    gz = data->gyro_z * deg2rad_factor;
    
    // printf("Gyro (rad/s): X=%.2f, Y=%.2f, Z=%.2f\n", gx, gy, gz);
    data->accel_x -= mpu_data_calibrated.accel_x;
    data->accel_y -= mpu_data_calibrated.accel_y;
    data->accel_z -= mpu_data_calibrated.accel_z;
    // 加速度计数据
    data->gyro_x -= mpu_data_calibrated.gyro_x;
    data->gyro_y -= mpu_data_calibrated.gyro_y;
    data->gyro_z -= mpu_data_calibrated.gyro_z;
    // 加速度归一化
    // printf("Accel: X=%.2f m/s², Y=%.2f m/s², Z=%.2f m/s²\n", data->accel_x, data->accel_y, data->accel_z);
    // printf("Gyro : X=%.2f °/s, Y=%.2f °/s, Z=%.2f °/s\n", data->gyro_x, data->gyro_y, data->gyro_z);
    norm = sqrtf(data->accel_x * data->accel_x + data->accel_y * data->accel_y + data->accel_z * data->accel_z);
    if (norm == 0.0f) return;
    ax = data->accel_x / norm;
    ay = data->accel_y / norm;
    az = data->accel_z / norm;
    // printf("Normalized Accel: X=%.2f, Y=%.2f, Z=%.2f\n", ax, ay, az);
    /**低通滤波 */
    if(flag_first){
        mahony_data.ax_filt = ax;
        mahony_data.ay_filt = ay;
        mahony_data.az_filt = az;
        flag_first = 0;
    }
    mahony_data.ax_filt = (1 - mahony_data.alpha) * mahony_data.ax_filt + mahony_data.alpha * ax;
    mahony_data.ay_filt = (1 - mahony_data.alpha) * mahony_data.ay_filt + mahony_data.alpha * ay;
    mahony_data.az_filt = (1 - mahony_data.alpha) * mahony_data.az_filt + mahony_data.alpha * az;

    ax = mahony_data.ax_filt;
    ay = mahony_data.ay_filt;
    az = mahony_data.az_filt;

    // 重力方向估计
    float q0 = mahony_data.q0, q1 = mahony_data.q1, q2 = mahony_data.q2, q3 = mahony_data.q3;
    float vx = 2.0f * (q1*q3 - q0*q2);
    float vy = 2.0f * (q0*q1 + q2*q3);
    float vz = 1.0f - 2.0f * (q1*q1 + q2*q2);
    // printf("Estimated Gravity: X=%.2f, Y=%.2f, Z=%.2f\n", vx, vy, vz);
    // 误差计算
    mahony_data.ex = (ay * vz - az * vy);
    mahony_data.ey = (az * vx - ax * vz);
    mahony_data.ez = (ax * vy - ay * vx);
    // printf("Error: ex=%.4f, ey=%.4f, ez=%.4f\n", mahony_data.ex, mahony_data.ey, mahony_data.ez);

    
    // 积分误差
    mahony_data.accex += mahony_data.k1 * mahony_data.ex * dt;
    mahony_data.accey += mahony_data.k1 * mahony_data.ey * dt;
    mahony_data.accez += mahony_data.k1 * mahony_data.ez * dt;

    // 积分限幅
    mahony_data.accex = fminf(fmaxf(mahony_data.accex, -mahony_data.integral_limit), mahony_data.integral_limit);
    mahony_data.accey = fminf(fmaxf(mahony_data.accey, -mahony_data.integral_limit), mahony_data.integral_limit);
    mahony_data.accez = fminf(fmaxf(mahony_data.accez, -mahony_data.integral_limit), mahony_data.integral_limit);

    // 补偿
    gx += mahony_data.k2 * mahony_data.ex + mahony_data.accex;
    gy += mahony_data.k2 * mahony_data.ey + mahony_data.accey;
    gz += mahony_data.k2 * mahony_data.ez + mahony_data.accez;


    // 四元数积分
    gx *= 0.5f * dt;
    gy *= 0.5f * dt;
    gz *= 0.5f * dt;

    mahony_data.q0 += (-q1 * gx - q2 * gy - q3 * gz);
    mahony_data.q1 += (q0 * gx + q2 * gz - q3 * gy);
    mahony_data.q2 += (q0 * gy - q1 * gz + q3 * gx);
    mahony_data.q3 += (q0 * gz + q1 * gy - q2 * gx);
    // printf("Quaternion before normalization: q0=%.4f, q1=%.4f, q2=%.4f, q3=%.4f\n", mahony_data.q0, mahony_data.q1, mahony_data.q2, mahony_data.q3);
    // 四元数归一化
    norm = sqrtf(mahony_data.q0 * mahony_data.q0 + mahony_data.q1 * mahony_data.q1 +
                 mahony_data.q2 * mahony_data.q2 + mahony_data.q3 * mahony_data.q3);
    if (norm > 0.0f) {
        mahony_data.q0 /= norm;
        mahony_data.q1 /= norm;
        mahony_data.q2 /= norm;
        mahony_data.q3 /= norm;
    }

    // 欧拉角计算
    q0 = mahony_data.q0; q1 = mahony_data.q1; q2 = mahony_data.q2; q3 = mahony_data.q3;

    imu_data.roll = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2)) * rad2deg_factor;

    float sinp = 2.0f*(q0*q2 - q3*q1);
    if (sinp > 1.0f) sinp = 1.0f;
    else if (sinp < -1.0f) sinp = -1.0f;
    imu_data.pitch = asinf(sinp) * rad2deg_factor;

    imu_data.yaw = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3)) * rad2deg_factor;
    // 输出结果
    // printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", imu_data.pitch, imu_data.roll, imu_data.yaw);
}


// void Time2_init(void)
// {
//     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM2, ENABLE);
//     TIM_TimeBaseInitTypeDef  timer_init_structure;
//     timer_init_structure.TIM_ClockDivision = TIM_CKD_DIV1;
//     timer_init_structure.TIM_CounterMode = TIM_CounterMode_Up;
//     timer_init_structure.TIM_Period = 1000 -1;
//     timer_init_structure.TIM_Prescaler = 72 -1;
//     timer_init_structure.TIM_RepetitionCounter = 0;
//     TIM_TimeBaseStructInit(&timer_init_structure);

// }
static MPU6050_Data mpu_data;
// uint8_t imu_update_flag = 0;
// void TIMER1_IRQHandler(void)
// {
//     if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_UP) == SET){
//         timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_UP);
//         imu_update_flag = 1; // 设置标志位
//         // mpu6050_get_data();	
// 		// data_get_average(10); // 计算平均值
//         // mpu_data = *mpu6050_get_raw_data();
//         // MahonyUpdate(&mpu_data, 0.01f);
//         // printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", imu_data.pitch, imu_data.roll, imu_data.yaw);
//     } 
// }

void data_get_average(MPU6050_Data* data, MPU6050_Data* raw_data, int count)
{
    if (count <= 0) return;

    raw_data->accel_x = (raw_data->accel_x * (count - 1) + data->accel_x) / count;
    raw_data->accel_y = (raw_data->accel_y * (count - 1) + data->accel_y) / count;
    raw_data->accel_z = (raw_data->accel_z * (count - 1) + data->accel_z) / count;

    raw_data->gyro_x = (raw_data->gyro_x * (count - 1) + data->gyro_x) / count;
    raw_data->gyro_y = (raw_data->gyro_y * (count - 1) + data->gyro_y) / count;
    raw_data->gyro_z = (raw_data->gyro_z * (count - 1) + data->gyro_z) / count;

    raw_data->temp = (raw_data->temp * (count - 1) + data->temp) / count;
    // printf("Average Accel: X=%.2f, Y=%.2f, Z=%.2f\n", mpu_data_raw.accel_x, mpu_data_raw.accel_y, mpu_data_raw.accel_z);
    // printf("Average Gyro : X=%.2f, Y=%.2f, Z=%.2f\n", mpu_data_raw.gyro_x, mpu_data_raw.gyro_y, mpu_data_raw.gyro_z);
    // printf("Average Temp : %.2f\n", mpu_data_raw.temp);
}

MPU6050_Data* mpu6050_get_raw_data()
{
    return &mpu_data_raw;
}   
MPU6050_Data* mpu6050_get_calibrated_data()
{
    return &mpu_data_calibrated;
}

MPU6050_Data* mpu6050_get_bias_data()
{
    mpu_data_bias.accel_x = mpu_data.accel_x - mpu_data_calibrated.accel_x;
    mpu_data_bias.accel_y = mpu_data.accel_y - mpu_data_calibrated.accel_y;
    mpu_data_bias.accel_z = mpu_data.accel_z - mpu_data_calibrated.accel_z;
    mpu_data_bias.gyro_x = mpu_data.gyro_x - mpu_data_calibrated.gyro_x;
    mpu_data_bias.gyro_y = mpu_data.gyro_y - mpu_data_calibrated.gyro_y;
    mpu_data_bias.gyro_z = mpu_data.gyro_z - mpu_data_calibrated.gyro_z;
    return &mpu_data_bias;
}

uint8_t mpu6050_calbration()
{
    for (int i = 0; i < 100; i++) {
        mpu6050_get_data(&mpu_data);
        mpu_data_calibrated.accel_x += mpu_data.accel_x;
        mpu_data_calibrated.accel_y += mpu_data.accel_y;
        mpu_data_calibrated.accel_z += mpu_data.accel_z;
        delay_ms(5);
    }
    mpu_data_calibrated.accel_x /= 100.0f;
    mpu_data_calibrated.accel_y /= 100.0f;
    mpu_data_calibrated.accel_z /= 100.0f;

    printf("Calibration offsets: X=%.2f, Y=%.2f, Z=%.2f\n", mpu_data_calibrated.accel_x, mpu_data_calibrated.accel_y, mpu_data_calibrated.accel_z);
    return 1;
}

IMU_data* imu_get_data()
{
    imu_data.yaw = 0.0f; // 假设没有磁力计数据，yaw保持为0
    return &imu_data;
}


void imu_task(void *pvParameters)
{
    while(1)
    {
        static uint8_t count = 0;
        mpu6050_get_data(&mpu_data);
        mpu6050_get_bias_data();
        data_get_average(&mpu_data_bias, &mpu_data_raw, 10); // 计算平均值
        if(++count%5==0){
            MahonyUpdate(&mpu_data_raw, 0.01f);
        }
        IMU_data* imu_data = {0};
        imu_data = imu_get_data();
        taskENTER_CRITICAL();       
        if(++count%500==0){
            printf("pitch: %.4f,roll:%.4f\r\n",imu_data->pitch, imu_data->roll);  
            count = 0;
        }    
        taskEXIT_CRITICAL();         
        vTaskDelay(2);
    }
}   